package clubhouse.base;

import java.util.Vector;
import clubhouse.motioncommand.CalculInfo;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class Robot {
	/**The X coordinate of the current robot position.*/
	private double X;
	/**The Y coordinate of the current robot position.*/
	private double Y; 
	/**The current robot direction in rad, r = 0 means the direction is the x-axis, r follows 
	 * the trigonometric direction.*/
	private double r;

	/**Set of all previous and current robot position.*/
	private Vector<double[]> position;
	/**Set of all previous and current robot position in an EKF trajectory.*/
	private Vector<double[]> estimatePosition;
	/**Set of all previous and current robot position in an error trajectory.*/
	private Vector<double[]> errorPosition;
	/**Set of all previous and current robot position in an Anti-SLAM trajectory.*/
	private Vector<double[]> antislamPosition;
	
	/**
	 * Construct the Robot object. Set the specified position as initial position and instantiate position's vectors.
	 * @param currentX the current robot X coordinate.
	 * @param currentY the current robot Y coordinate.
	 * @param currentr the current robot direction in rad, r = 0 means the direction is the x-axis, r follows 
	 * the trigonometric direction.
	 */ 
	public Robot(double currentX, double currentY, double currentr) {
		this.X = currentX;
		this.Y = currentY;
		this.r = currentr;
		
		this.position = new Vector<double[]>(100,10);
		this.estimatePosition = new Vector<double[]>(100,10);
		this.errorPosition = new Vector<double[]>(100,10);
		this.antislamPosition = new Vector<double[]>(100,10);
		
		this.storePosition();
	}
	
	/**
	 * Get the current robot X coordinate.
	 * @return the current X coordinate.
	 */
	public double getX() {
		return X;
	}
	
	/**
	 * Get the current robot Y coordinate.
	 * @return the current Y coordinate.
	 */
	public double getY() {
		return Y;
	}
	
	/**
	 * Get the current robot direction.
	 * @return the current direction in rad.
	 */
	public double getr() {
		return r;
	}
	
	/**
	 * Get the last position of the robot.
	 * @return the double array containing the last position [X coordinate, Y coordinate, direction].
	 */
	public double[] getLastPosition() {
		return position.lastElement();
	}
	
	/**
	 * Set the new robot position as specified.
	 * @param currentX the new robot X coordinate.
	 * @param currentY the new robot Y coordinate.
	 * @param currentr the new robot direction in rad.
	 */
	public void setPosition(double currentX, double currentY, double currentr) {
		this.X = currentX;
		this.Y = currentY;
		this.r = currentr;
	}
	
	/**
	 * Store the current position in the position vector.
	 */
	private void storePosition() {
		this.position.add(new double[] {X,Y,r});
	}
	
	/**
	 * Store the current position in the estimate position vector.
	 */
	private void storeEstimatePosition() {
		this.estimatePosition.add(new double[] {X,Y,r});
	}
	
	/**
	 * Store the current position in the error position vector.
	 */
	private void storeErrorPosition() {
		this.errorPosition.add(new double[] {X,Y,r});
	}
	
	/**
	 * Store the current position in the Anti-SLAM position vector.
	 */
	private void storeAntislamPosition() {
		this.antislamPosition.add(new double[] {X,Y,r});
	}
	
	/**
	 * Store the current position in the correct vector according to the specified trajectory.
	 * <br/>Remark : This function and these vectors are also used during Anti-SLAM comparison even if parameters are not the same.
	 * <br/>In this case, position vector corresponds to trajectory without Anti-SLAM, error position vector corresponds to Translation Anti-SLAM strategy,
	 * estimate position vector corresponds to Rotation Anti-SLAM strategy and Anti-SLAM vector corresponds to Hybrid Anti-SLAM strategy.
	 * @param trajectory equals to one of the "Complete Simulation Constants" according to the current trajectory.
	 */
	public void storeCorrectPosition(int trajectory){
		switch (trajectory){
			case Base.INITIALTRAJECTORY:
				this.storePosition();
				break;
			case Base.ERRORTRAJECTORY:
				this.storeErrorPosition();
				break;
			case Base.EKFTRAJECTORY:
				this.storeEstimatePosition();
				break;
			case Base.ANTISLAMTRAJECTORY:
				this.storeAntislamPosition();								
				break;
			default:
				break;
		}
	}
	
	
	/**
	 * Get all stored robot positions. Used when the robot is not disturbed by error, Anti-SLAM or EKF.
	 * @return the whole position vector.
	 */
	public Vector<double[]> getPositions() {
		return this.position;
	}
	
	/**
	 * Get all stored robot estimate positions. Used when error and EKF filter are enabled.
	 * @return the whole estimate position vector.
	 */
	public Vector<double[]> getEstimatePositions() {
		return this.estimatePosition;
	}
	
	/**
	 * Get all stored robot error positions. Used when only error is enabled.
	 * @return the whole error position vector.
	 */
	public Vector<double[]> getErrorPositions() {
		return this.errorPosition;
	}
	
	/**
	 * Get all stored robot Anti-SLAMed positions. Used when error, EKF filter and Anti-SLAM are enabled.
	 * @return the whole Anti-SLAM position vector.
	 */
	public Vector<double[]> getAntislamPositions() {
		return this.antislamPosition;
	}
	
	/**
	 * Set the new position calculated from specified motion's pulses.
	 * @param pulseDist the number of pulses in distance.
	 * @param pulseRotation the number of pulses in rotation.
	 * @param trigo the rotation direction, 1 if trigonometric direction, 0 otherwise.
	 */
	public void setNewPosition(double pulseDist, double pulseRotation, int trigo){
		double [] temp = new double[3];
		if (trigo==0) pulseRotation = -pulseRotation;
		
		temp[2] = this.r + pulseRotation/CalculInfo.omega;
		if (temp[2] <= -Math.PI) temp[2] = temp[2] + 2*Math.PI;
		if (temp[2] > Math.PI) temp[2] = temp[2] - 2*Math.PI;
		temp[0] = this.X + (pulseDist/CalculInfo.delta)*Math.cos(temp[2]);
		temp[1] = this.Y + (pulseDist/CalculInfo.delta)*Math.sin(temp[2]);
		
		this.setPosition(temp[0], temp[1], temp[2]);
	}
}
